﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;
using System.IO;
using BobcatUtility;
using MouseKeyboardLibrary;

namespace BobcatWiiLib
{
    /// <summary>
    /// Manages IR inputs from Wiimotes and makes the sources available via events. You are 
    /// notified when a source is turned on or off and when the position of a source changes.
    /// This class can also optionally raise mouse-click and mouse-move events when IR sources move.
    /// </summary><remarks>
    /// The positions are calibrated to screen coordinates, so the calibration must be done 
    /// before any events will be fired. This can be done by:
    /// - Calling the showCalibrationScreen() method.
    /// - Calling setCalibrationData(strData)
    /// 
    /// After calibration has occured you can store the calibration data by calling getCalibrationData() 
    /// and storing the returned string yourself (e.g. in a database).
    /// 
    /// Alternatively, if you want to use a file to store calibration data, you can use the 
    /// helper function loadCalibrationData(strFilename). This loads calibration data from the
    /// file. If the file does not exist, it shows the calibration screen, performs the 
    /// calibration and then stores the data in the file for next time.
    /// 
    /// Note1: The coordinates that are passed back with events are screen coordinates. If you
    ///        want the coordinates realtive to a particular window you can call PointToClient 
    ///        to convert them.
    ///         
    /// Note2: If you receive events from this class in a form or control, the events will be 
    ///        marshaled onto the form's thread. But data in this class may be being updated by
    ///        a background thread, so this data is synchronized by a lock.
    ///        
    /// Raising mouse events
    /// --------------------
    /// If the property RaiseMouseEvents is set to true, then we raise mouse events based on the
    /// IR events we're managing. Mouse events will only be raised if there is a single IR source
    /// detected.
    /// </remarks>
    public class CalibratedIRManager
    {
        #region Events

        /// <summary>
        /// Data passed to events.
        /// </summary>
        public class IRArgs : System.EventArgs
        {
            public int Source;
            public bool State;
            public Point ScreenPoint = new Point();
        }

        /// <summary>
        /// Fires when a source is turned on or off.
        /// </summary>
        public event EventHandler<IRArgs> IRStateChangedEvent;

        /// <summary>
        /// Fired when an IR source changes position.
        /// </summary>
        public event EventHandler<IRArgs> IRPositionChangedEvent;

        /// <summary>
        /// Fires when the IR config changes. (The params are more-or-less irrelevant in this case.)
        /// </summary>
        public event EventHandler<IRArgs> ConfigChangedEvent;

        #endregion

        #region Public properties

        /// <summary>
        /// Property to get and set the smoothing level.
        /// </summary>
        public int SmoothingLevel
        {
            get 
            {
                return m_iSmoothingLevel; 
            }
            set
            {
                // We set the smoothing level for the wiimotes we manage...
                m_iSmoothingLevel = value;
                m_Wiimote.SmoothingLevel = m_iSmoothingLevel;

                // We raise the event to notify any obersers that the config may have changed...
                Utility.CrossThreadInvoke(ConfigChangedEvent, this, new IRArgs());
            }
        }

        /// <summary>
        /// Property to get or set the  image used for the calibration screen.
        /// </summary>
        public string CalibrationImageFilename
        {
            get
            {
                return m_strCalibrationImageFilename;
            }
            set
            {
                m_strCalibrationImageFilename = value;

                // We raise the event to notify any obersers that the config may have changed...
                Utility.CrossThreadInvoke(ConfigChangedEvent, this, new IRArgs());
            }
        }

        /// <summary>
        /// Property to control whether mouse events are raised from their corresponding
        /// IR events.
        /// </summary>
        public bool RaiseMouseEvents
        {
            get
            {
                return m_bRaiseMouseEvents;
            }
            set
            {
                m_bRaiseMouseEvents = value;

                // We raise the event to notify any obersers that the config may have changed...
                Utility.CrossThreadInvoke(ConfigChangedEvent, this, new IRArgs());
            }
        }

        #endregion

        #region Public methods

        /// <summary>
        /// Constructor.
        /// </summary>
        public CalibratedIRManager()
        {
            // We find the wiimote. At the moment this class just supports one wiimote.
            // We use WiimoteLib to find the first wiimote and create a wrapper object 
            // to help manage it...
            WiimoteLib.WiimoteCollection wiimotes = new WiimoteLib.WiimoteCollection();
            wiimotes.FindAllWiimotes();
            if (wiimotes.Count == 0)
            {
                throw new Exception("There are no wiimotes currently connected.");
            }

            // We connect to the first wiimote...
            WiimoteLib.Wiimote wiimote = wiimotes[0];
            wiimote.Connect();
            wiimote.SetReportType(WiimoteLib.InputReport.IRAccel, true);
            wiimote.SetLEDs(false, true, false, true);

            m_Wiimote = new WiimoteWrapper(wiimote);
            m_Wiimote.connect();

            // We register for events from the wiimote...
            m_Wiimote.IRStatusChangedEvent += onSourceStateChanged;
            m_Wiimote.IRPositionChangedEvent += onSourcePositionChanged;

            // We create 4 source-infos to hold data for the points we can track...
            m_SourceInfos.Add(new SourceInfo { iSourceIndex = 0 });
            m_SourceInfos.Add(new SourceInfo { iSourceIndex = 1 });
            m_SourceInfos.Add(new SourceInfo { iSourceIndex = 2 });
            m_SourceInfos.Add(new SourceInfo { iSourceIndex = 3 });

            // We set a default smoothing-level...
            SmoothingLevel = 3;
        }

        /// <summary>
        /// Shows a calibration dialog and then calculates the calibration transformation between
        /// Wiimote coordinates and screen coordinates.
        /// </summary>
        public void showCalibrationScreen()
        {
            // We don't want to raise mouse events while in the calibration screen, as the 
            // screen may not be calibrated and we would raise them into the wrong place...
            bool bRaiseMouseEvents = m_bRaiseMouseEvents;
            m_bRaiseMouseEvents = false;

            // We show the calibration screen...
            CalibrationForm form = new CalibrationForm(m_Wiimote, m_strCalibrationImageFilename);
            form.ShowDialog();

            // We reset raising of mouse events...
            m_bRaiseMouseEvents = bRaiseMouseEvents;

            // If the calibration completed there will be 4 calibration points...
            IList<CalibrationForm.CoordinatePair> coordinatePairs = form.getCoordinatePairs();
            if (coordinatePairs.Count != 4)
            {
                return;
            }
            calibrate(coordinatePairs);

            // We raise the event to notify any obersers that the config may have changed...
            Utility.CrossThreadInvoke(ConfigChangedEvent, this, new IRArgs());
        }

        /// <summary>
        /// Loads calibration data from a file, or shows the calibration screen if the file
        /// does not exist.
        /// </summary>
        public void loadCalibrationData(string strFilename)
        {
            // Does the file exist?
            bool bFileExists = File.Exists(strFilename);
            if (bFileExists == true)
            {
                // We load calibration data from the file...
                string strData = File.ReadAllText(strFilename);
                setCalibrationData(strData);
            }
            else
            {
                // We show the calibration screen, and then save the data to the file...
                showCalibrationScreen();
                File.WriteAllText(strFilename, m_strCalibrationPoints);
            }
        }


        /// <summary>
        /// Returns the calibration data. You can store this data, and later pass it to setCalibrationData to
        /// perform the calibration without showing the calibration screen.
        /// </summary>
        public string getCalibrationData()
        {
            return m_strCalibrationPoints;
        }


        /// <summary>
        /// We calibrate the IR points using the data passed in. (This was most likely retrieved after an
        /// earlier calibration.)
        /// </summary>
        public void setCalibrationData(string strData)
        {
            IList<CalibrationForm.CoordinatePair> coordinatePairs = convertStringToCalibrationPoints(strData);
            calibrate(coordinatePairs);
        }


        /// <summary>
        /// Returns info about the current set of active sources.
        /// </summary>
        /// <returns></returns>
        public IList<IRArgs> getActiveSources()
        {
            lock (m_Lock)
            {
                return getActiveSourcesImpl();
            }
        }


        /// <summary>
        /// Returns the list of active sources which are inside the rectangle passed in.
        /// The points passed in must be in screen coordinates.
        /// </summary>
        public IList<IRArgs> getActiveSources(Rectangle bounds)
        {
            lock (m_Lock)
            {
                return getActiveSourcesImpl(bounds);
            }
        }

        #endregion

        #region Private helper functions

        /// <summary>
        /// Performs the calibration using the points passed in.
        /// </summary>
        /// <param name="coordinatePairs"></param>
        private void calibrate(IList<CalibrationForm.CoordinatePair> coordinatePairs)
        {
            // We need 4 points...
            if(coordinatePairs.Count != 4)
            {
                throw new Exception("There must be 4 points in the calibration data.");
            }

            // We've got the calibration points, so we can perform the calibration...
            double[,] wiiPoints = new double[4, 2];
            wiiPoints[0, 0] = coordinatePairs[0].wiimoteX;
            wiiPoints[0, 1] = coordinatePairs[0].wiimoteY;
            wiiPoints[1, 0] = coordinatePairs[1].wiimoteX;
            wiiPoints[1, 1] = coordinatePairs[1].wiimoteY;
            wiiPoints[2, 0] = coordinatePairs[2].wiimoteX;
            wiiPoints[2, 1] = coordinatePairs[2].wiimoteY;
            wiiPoints[3, 0] = coordinatePairs[3].wiimoteX;
            wiiPoints[3, 1] = coordinatePairs[3].wiimoteY;

            double[,] screenPoints = new double[4, 2];
            screenPoints[0, 0] = coordinatePairs[0].screenX;
            screenPoints[0, 1] = coordinatePairs[0].screenY;
            screenPoints[1, 0] = coordinatePairs[1].screenX;
            screenPoints[1, 1] = coordinatePairs[1].screenY;
            screenPoints[2, 0] = coordinatePairs[2].screenX;
            screenPoints[2, 1] = coordinatePairs[2].screenY;
            screenPoints[3, 0] = coordinatePairs[3].screenX;
            screenPoints[3, 1] = coordinatePairs[3].screenY;

            m_FourPointCalibration = new four_point_callibration.four_point_calibration();
            m_FourPointCalibration.do_calibration(wiiPoints, screenPoints);

            // We store the calibration points in a text format...
            m_strCalibrationPoints = convertCalibrationPointsToString(coordinatePairs);
        }
        
        /// <summary>
        /// Converts the calibration points to a string format so that they can be stored.
        /// </summary>
        private string convertCalibrationPointsToString(IList<CalibrationForm.CoordinatePair> coordinatePairs)
        {
            string strResult = "";
            foreach (CalibrationForm.CoordinatePair coordinatePair in coordinatePairs)
            {
                strResult += coordinatePair.wiimoteX.ToString();
                strResult += ",";
                strResult += coordinatePair.wiimoteY.ToString();
                strResult += ",";
                strResult += coordinatePair.screenX.ToString();
                strResult += ",";
                strResult += coordinatePair.screenY.ToString();
                strResult += ";";
            }
            return strResult;
        }

        /// <summary>
        /// Converts a string into calibration points.
        /// </summary>
        private IList<CalibrationForm.CoordinatePair> convertStringToCalibrationPoints(string strData)
        {
            IList<CalibrationForm.CoordinatePair> results = new List<CalibrationForm.CoordinatePair>();

            // The (4) points are separated by ';'...
            string[] points = strData.Split(';');
            foreach (string point in points)
            {
                if (point == "") continue;

                // The point has 4 parts separated by ','...
                string[] coords = point.Split(',');
                if (coords.GetLength(0) != 4)
                {
                    throw new Exception("Point in calibration data must have 4 elements.");
                }
                CalibrationForm.CoordinatePair pair = new CalibrationForm.CoordinatePair();
                pair.wiimoteX = Convert.ToDouble(coords[0]);
                pair.wiimoteY = Convert.ToDouble(coords[1]);
                pair.screenX = Convert.ToDouble(coords[2]);
                pair.screenY = Convert.ToDouble(coords[3]);
                results.Add(pair);
            }

            return results;
        }


        /// <summary>
        /// Called when a point is turned on or off. We update our internal state and
        /// fire an event.
        /// </summary>
        private void onSourceStateChanged(object sender, WiimoteWrapper.IRPointArgs args)
        {
            lock (m_Lock)
            {
                onSourceStateChangedImpl(args);
            }
        }
        private void onSourceStateChangedImpl(WiimoteWrapper.IRPointArgs args)
        {
            // We do not handle events until the calibration has succeeded...
            if (m_FourPointCalibration == null)
            {
                return;
            }

            // We convert the point to a screen point...
            Point screenPoint = convertWiimoteCoordsToScreenPoint(args.PositionX, args.PositionY);

            // Has the point been switched on or off?
            SourceInfo sourceInfo = null;
            if (args.Active == true)
            {
                // The point has been turned on so we allocate it to the first inactive source...
                foreach (SourceInfo si in m_SourceInfos)
                {
                    if (si.bState == true)
                    {
                        continue;
                    }
                    // This source is inactive, so we assign the new point to it...
                    si.bState = true;
                    si.ScreenPoint = screenPoint;
                    sourceInfo = si;
                    break;
                }
            }
            else
            {
                // The point has been turned off, so we find the source and deactivate it...
                sourceInfo = findNearestSource(screenPoint);
                if (sourceInfo != null)
                {
                    sourceInfo.bState = false;
                }
            }

            // We fire an event to our observers, and we may need to raise a mouse-event as well...
            if (sourceInfo == null)
            {
                return;
            }
            Utility.CrossThreadInvoke(IRStateChangedEvent, this, getArgsFromSourceInfo(sourceInfo));
            RaiseMouseUpOrDownEvent(sourceInfo);
        }

        /// <summary>
        /// If the raise-mouse-events flag is set and there is only one active IR source, then
        /// we raise a mouse event at the current IR point.
        /// </summary>
        private void RaiseMouseUpOrDownEvent(SourceInfo sourceInfo)
        {
            // Are mouse events switched on?
            if (m_bRaiseMouseEvents == false)
            {
                return;
            }

            // We only raise mouse events if there is / was only one active source. This depends
            // on the event that has occurred...
            int iActiveSources = getNumberOfActiveSources();
            if (sourceInfo.bState == true)
            {
                // The point has become active. If there is only one active source, we
                // will raise a mouse-down event...
                if (iActiveSources == 1)
                {
                    MouseSimulator.Position = Utility.copy(sourceInfo.ScreenPoint);
                    MouseSimulator.MouseDown(MouseButton.Left);
                }
            }
            else
            {
                // The point has become inactive. If it was the only IR source we raise a
                // mouse-up event...
                if (iActiveSources == 0)
                {
                    MouseSimulator.Position = Utility.copy(sourceInfo.ScreenPoint);
                    MouseSimulator.MouseUp(MouseButton.Left);
                }
            }
        }

        /// <summary>
        /// Raises a mouse-move event if mouse-events are switched on and there is
        /// just one active source.
        /// </summary>
        private void RaiseMouseMoveEvent(SourceInfo sourceInfo)
        {
            // Are mouse events switched on?
            if (m_bRaiseMouseEvents == false)
            {
                return;
            }

            // Is there just one active source?
            int iActiveSources = getNumberOfActiveSources();
            if (iActiveSources != 1)
            {
                return;
            }

            // We move the mouse to the new position...
            MouseSimulator.Position = Utility.copy(sourceInfo.ScreenPoint);
        }

        /// <summary>
        /// Returns the number of active sources.
        /// </summary>
        private int getNumberOfActiveSources()
        {
            int iResult = 0;
            foreach (SourceInfo si in m_SourceInfos)
            {
                if (si.bState == true) iResult++;
            }
            return iResult;
        }

        /// <summary>
        /// Copies data from the sourceInfo into an IRArgs object that can be passed with an event.
        /// </summary>
        /// <param name="sourceInfo"></param>
        /// <returns></returns>
        private IRArgs getArgsFromSourceInfo(SourceInfo sourceInfo)
        {
            IRArgs result = new IRArgs();
            if (sourceInfo != null)
            {
                result.Source = sourceInfo.iSourceIndex;
                result.State = sourceInfo.bState;
                result.ScreenPoint.X = sourceInfo.ScreenPoint.X;
                result.ScreenPoint.Y = sourceInfo.ScreenPoint.Y;
            }
            return result;
        }


        /// <summary>
        /// Called when a point's position is updated from the wiimote. We work out which of
        /// our active sources it is, update it and fire an event.
        /// </summary>
        private void onSourcePositionChanged(object sender, WiimoteWrapper.IRPointArgs args)
        {
            lock (m_Lock)
            {
                onSourcePositionChangedImpl(args);
            }
        }
        private void onSourcePositionChangedImpl(WiimoteWrapper.IRPointArgs args)
        {
            // We do not handle events until the calibration has succeeded...
            if (m_FourPointCalibration == null)
            {
                return;
            }

            // We convert the point to screen coords and find the nearest active
            // source that we are managing...
            Point screenPoint = convertWiimoteCoordsToScreenPoint(args.PositionX, args.PositionY);
            SourceInfo sourceInfo = findNearestSource(screenPoint);
            if (sourceInfo == null)
            {
                return;
            }

            // We update the point and raise the event...
            sourceInfo.ScreenPoint = screenPoint;
            Utility.CrossThreadInvoke(IRPositionChangedEvent, this, getArgsFromSourceInfo(sourceInfo));
            RaiseMouseMoveEvent(sourceInfo);
        }


        /// <summary>
        /// Returns the SourceInfo for the active source we are managing which is nearest to
        /// the point passed in. The idea of this is that we try to keep the same source indexes
        /// for our points even if the indexes used by the wiimote itself change.
        /// </summary>
        private SourceInfo findNearestSource(Point screenPoint)
        {
            SourceInfo result = null;

            // We look through the source info we've got, and compare the point
            // passed in with the points for all active sources...
            double dNearestDistance = 0.0;
            foreach (SourceInfo sourceInfo in m_SourceInfos)
            {
                if (sourceInfo.bState == false)
                {
                    // This source is not active...
                    continue;
                }

                // We've got an active source. Is it the nearest? We find the distance to this source.
                double dDeltaX = sourceInfo.ScreenPoint.X - screenPoint.X;
                double dDeltaY = sourceInfo.ScreenPoint.Y - screenPoint.Y;
                double dDistance = dDeltaX * dDeltaX + dDeltaY * dDeltaY;

                if (result == null || dDistance < dNearestDistance)
                {
                    // We've found a new nearest point...
                    result = sourceInfo;
                    dNearestDistance = dDistance;
                }
            }

            return result;
        }


        /// <summary>
        /// Converts coordinates received from the wiimote to screen coordinates.
        /// </summary>
        private Point convertWiimoteCoordsToScreenPoint(double dWiimoteX, double dWiimoteY)
        {
            double dScreenX = m_FourPointCalibration.get_x(dWiimoteX, dWiimoteY);
            double dScreenY = m_FourPointCalibration.get_y(dWiimoteX, dWiimoteY);

            Point result = new Point();
            result.X = (int)Math.Round(dScreenX);
            result.Y = (int)Math.Round(dScreenY);

            return result;
        }


        /// <summary>
        /// Returns a collection of IRArgs for the active sources.
        /// </summary>
        private IList<IRArgs> getActiveSourcesImpl()
        {
            IList<IRArgs> results = new List<IRArgs>();
            foreach (SourceInfo si in m_SourceInfos)
            {
                // Is the source active?
                if (si.bState == false)
                {
                    continue;
                }

                // The source is one we want, so we add it to the results...
                results.Add(getArgsFromSourceInfo(si));
            }
            return results;
        }


        /// <summary>
        /// Returns the active sources (as IRArgs) that fall within the bounds
        /// rectangle passed in.
        /// </summary>
        private IList<IRArgs> getActiveSourcesImpl(Rectangle bounds)
        {
            IList<IRArgs> results = new List<IRArgs>();
            foreach (SourceInfo si in m_SourceInfos)
            {
                // Is the source active?
                if (si.bState == false)
                {
                    continue;
                }

                // Is the source inside the bounds passed in?
                if (si.ScreenPoint.X < bounds.Left
                   ||
                   si.ScreenPoint.X > bounds.Right
                   ||
                   si.ScreenPoint.Y < bounds.Top
                   ||
                   si.ScreenPoint.Y > bounds.Bottom)
                {
                    continue;
                }

                // The point is one we want, so we add it to the results...
                results.Add(getArgsFromSourceInfo(si));
            }
            return results;
        }

        #endregion

        #region Private data

        // Object that manages the wiimote we get the data from...
        private WiimoteWrapper m_Wiimote;

        // Smoothing level to use for the wiimotes we manage...
        private int m_iSmoothingLevel;

        // Class that manages the converrsion between wiimote and screen coordinates...
        private four_point_callibration.four_point_calibration m_FourPointCalibration;

        // We store a collection of source-info objects, with data about each source we're tracking.
        private class SourceInfo
        {
            public int iSourceIndex;
            public bool bState;
            public Point ScreenPoint = new Point();
        }
        private IList<SourceInfo> m_SourceInfos = new List<SourceInfo>();
        private object m_Lock = new object();

        // String representation of calibration points...
        string m_strCalibrationPoints;

        // The image filename used for the calibration screen...
        private string m_strCalibrationImageFilename = "";

        // Flag saying whether we raise mouse events...
        private bool m_bRaiseMouseEvents = false;

        #endregion
    }
}
